// -----------------------------
// Author: 		Dhaenens Gert	
// Creation: 	1/6/2015 10:28:28 AM				
// -----------------------------

#include "LowPass.h"

#include "Oscillator.h"

LowPass::LowPass()
{
}

LowPass::~LowPass()
{
}

SoundSample LowPass::Apply(const SoundSample& input) const
{
	Oscillator* lpOsc = nullptr;
	if (Modulate)
		lpOsc = Oscillator::Create(ModulateType);

	// Create the new Sample
	SoundSample sample = {};
	sample.Channels = input.Channels;
	sample.SampleRate = input.SampleRate;
	sample.Count = input.Count;
	sample.Samples = new Float[sample.Count];
	memset(sample.Samples, 0, sample.Count * sizeof(Float));

	// Calculate the Pass
	UInt32 baseOffset = 2;
	for (UInt32 i = baseOffset; i < (sample.Count / sample.Channels); ++i)
	{

		// Calculate the Local Values
		Float fc = CutoffFrequency;
		if (Modulate)
		{
			Float t = (Float)i / sample.SampleRate;
			fc = ModulateFreq(lpOsc, t);
		}
		Float fs = (Float)input.SampleRate;

		Float w = Math::PiTimes2 * fc / fs;
		Float d = 1.0f / QualityFactor;
		Float beta = ((1.0f - (d / 2.0f) * Math::Sin(w)) / (1.0f + (d / 2.0f) * Math::Sin(w))) / 2.0f;
		Float gamma = (0.5f + beta) * Math::Cos(w);

		// Calculate the Coefficients
		Float a0 = (0.5f + beta - gamma) / 2.0f;
		Float a1 = (0.5f + beta - gamma);
		Float a2 = a0;
		Float b1 = -2.0f * gamma;
		Float b2 = 2.0f * beta;

		Float inSample = input.Samples[i * input.Channels];
		Float prevInSample = input.Samples[(i - 1) * input.Channels];
	
		Float lastOutSample = sample.Samples[(i - 1) * sample.Channels];
		Float delayOutSample = sample.Samples[(i - 2) * sample.Channels];
	
		Float outSample = (a0 * inSample) + (a1 * prevInSample) - (b1 * lastOutSample) - (b2 * delayOutSample);
		for (UInt32 c = 0; c < sample.Channels; ++c)
		{
			UInt32 idx = (i * sample.Channels) + c;
			sample.Samples[idx] = outSample * Gain;
		}
	}

	delete lpOsc;

	return sample;
}

Float LowPass::ModulateFreq(Oscillator* lpOsc, Float t) const
{
	lpOsc->Amplitude = ModulateAmplitude;
	lpOsc->Frequency = ModulateFrequency;
	lpOsc->Harmonics = 20;

	Float v = lpOsc->Sample(t);
	Int32 centOffset = (Int32)(v * ModulateStrength);
	Float freqOffset = SoundSample::CentsToFreqScale(centOffset);

	return (CutoffFrequency * freqOffset);
}